#include <cmath>
#include "zhnmat.hpp"
#include "algorithm.hpp"
#include "utils.cpp"
using namespace std;
using namespace zhnoptim;
using namespace zhnmat;

vector<Vector3d> pixels, coords;

double Cost_Function(const vector<double>& a)
{
    double theta = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
    Mat n(vector<double>{a[0]/theta, a[1]/theta, a[2]/theta});
    Mat R = cos(theta)*eye(3) + (1-cos(theta))*n*n.T() + sin(theta)*n.M();
    Vector3d y, errvec;
    double cost = 0;
    for (int i=pixels.size()-1; i>=0; --i){
        y = R*pixels[i] + 100*Vector3d(a[3], a[4], a[5]);
        errvec = y - coords[i];
        cost += errvec*errvec;
    }
    return cost/pixels.size();
}

int main()
{
    // 指定点在左相机坐标系下的坐标
    pixels.push_back(Vector3d(36.71758639830871, -37.33281611510293, 136.4769250938883));
    pixels.push_back(Vector3d(9.002102780054246, -58.56992832606053, 152.2570695578691));
    pixels.push_back(Vector3d(-12.06782614023492, 7.961551256175206, 243.6113112925906));
    pixels.push_back(Vector3d(16.67220268444801, -64.68491690514342, 270.6792347695451));
    pixels.push_back(Vector3d(79.4140584801529, -53.78159583400829, 188.1168426969812 ));
    pixels.push_back(Vector3d(88.17325565053457, 26.63106102111433, 206.4502638072802 ));
    pixels.push_back(Vector3d(65.94153255770064, 45.63635407650149, 188.1168426969812 ));
    // 指定点在世界坐标系下的坐标
    coords.push_back(Vector3d(-118.281,329.514,266.77 ));
    coords.push_back(Vector3d(-136.781,304.618,284.219));
    coords.push_back(Vector3d(-219.043,301.197,213.267));
    coords.push_back(Vector3d(-243.99,329.163,281.322 ));
    coords.push_back(Vector3d(-160.926,376.805,281.322));
    coords.push_back(Vector3d(-168.532,390.923,203.27 ));
    coords.push_back(Vector3d(-155.777,368.468,184.047));
    vector<double> best{-1.2953796, -1.1787769, 1.025057, 0.18968234, 2.7422776, 2.3831151};
//    Differential_Evolution de(best, 6, 90);
//    de.Set_CostFunction(Cost_Function);
//    de.run();
//    best = de.Get_BestSolution();
    Pattern_Search ps(best, 6, 0.125);
    ps.Set_CostFunction(Cost_Function);
    ps.run();
    double theta = sqrt(best[0]*best[0] + best[1]*best[1] + best[2]*best[2]);
    Mat::OutputFormat = 0;
    best = ps.Get_BestSolution();
    Mat n(vector<double>{best[0]/theta, best[1]/theta, best[2]/theta});
    Mat R = cos(theta)*eye(3) + (1-cos(theta))*n*n.T() + sin(theta)*n.M();
    Mat T(vector<double>{best[3], best[4], best[5]});
    PRINT_NAME_VALUE(R);
    PRINT_NAME_VALUE(T);
    return 0;
}
